/* USER CODE BEGIN Header */
/**
  ******************************************************************************
  * @file           : main.c
  * @brief          : Main program body
  ******************************************************************************
  * @attention
  *
  * Copyright (c) 2024 STMicroelectronics.
  * All rights reserved.
  *
  * This software is licensed under terms that can be found in the LICENSE file
  * in the root directory of this software component.
  * If no LICENSE file comes with this software, it is provided AS-IS.
  *
  ******************************************************************************
  */
/* USER CODE END Header */
/* Includes ------------------------------------------------------------------*/
#include "main.h"
#include "dma.h"
#include "tim.h"
#include "usart.h"
#include "gpio.h"

/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include "stdio.h"
#include "stepper_motor.h"
#include "visul_communicate.h"
#include "abs_encoder.h"
/* USER CODE END Includes */

/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN PTD */
#define cnt_clk 168000000/(167+1)//计数器频率
#define arr 65535//自定重装载值  
#define pulse_half 1000
#define high_M_1_pluse 320
#define high_M_N 2
#define radian_M_1_pulse 200
#define radian_M_N 30
#define high_M_1_circle 0.032 //高度上下1cm对应的电机圈数行程
//高度电机最大活动范围 77cm - 99cm
#define circle_max_pulse 1200

uint32_t T3_CCR1, T3_CCR2, T3_end_flag;//存捕获寄存器获取的的变量
float T3_duty_cycle,T3_frequency;//频率，占空比
uint32_t T4_CCR1, T4_CCR2, T4_end_flag;//存捕获寄存器获取的的变量
float T4_duty_cycle,T4_frequency;//频率，占空比
uint8_t object_pos_ctr_flag = 0;
int16_t input_pulse = 0;

__IO uint32_t g_set_speed  = 20;          /* 最大速度 单位为0.1rad/sec */
__IO uint32_t g_step_accel = 10;          /* 加速度 单位为0.1rad/sec^2 */
__IO uint32_t g_step_decel = 10;          /* 减速度 单位为0.1rad/sec^2 */
__IO uint16_t g_step_angle = 10;          /* 设置的步数*/
#define radian_speed_add 80
#define radian_step_add 80
#define radian_accel_add 40
#define radian_decel_add 40
#define zero_speed_reduce 10
#define zero_step_reduce 5
extern speedRampData g_srd1;
extern speedRampData g_srd2;
extern speedRampData g_srd3;
extern speedRampData g_srd4;
extern UART_HandleTypeDef huart2;
extern UART_HandleTypeDef huart3;
uint8_t high_angctr_state = 0;
uint8_t visual_angctr_state = 0;
uint8_t zero_find_left_flag = 0;
uint8_t zero_find_right_flag = 0;

uint8_t zero1_state = 0;
uint8_t zero2_state = 0;
uint64_t zero1_1_count = 0;
uint64_t zero1_0_count = 0;
uint64_t zero2_1_count = 0;
uint64_t zero2_0_count = 0;

extern uint64_t g_step_position1;
extern uint64_t g_step_position2;
extern uint64_t g_step_position3;
extern uint64_t g_step_position4;

extern float ctr_value_flo;
extern float targer_circle;
extern uint8_t Ctr_high_M_flag;
float fake_value_flo_arr[] = {1.2, -2.5, 0.8, -1.1, 2.6, -1.0};
float fake_value_flo;

extern float real_pos_L;
extern float real_pos_R;

/* USER CODE END PTD */

/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */

/* USER CODE END PD */

/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */

/* USER CODE END PM */

/* Private variables ---------------------------------------------------------*/

/* USER CODE BEGIN PV */

/* USER CODE END PV */

/* Private function prototypes -----------------------------------------------*/
void SystemClock_Config(void);
/* USER CODE BEGIN PFP */

/* USER CODE END PFP */

/* Private user code ---------------------------------------------------------*/
/* USER CODE BEGIN 0 */

/* USER CODE END 0 */

/**
  * @brief  The application entry point.
  * @retval int
  */
int main(void)
{
  /* USER CODE BEGIN 1 */

  /* USER CODE END 1 */

  /* MCU Configuration--------------------------------------------------------*/

  /* Reset of all peripherals, Initializes the Flash interface and the Systick. */
  HAL_Init();

  /* USER CODE BEGIN Init */

  /* USER CODE END Init */

  /* Configure the system clock */
  SystemClock_Config();

  /* USER CODE BEGIN SysInit */

  /* USER CODE END SysInit */

  /* Initialize all configured peripherals */
  MX_GPIO_Init();
  MX_DMA_Init();
  MX_TIM8_Init();
  MX_TIM2_Init();
  MX_TIM3_Init();
  MX_TIM4_Init();
  MX_USART1_UART_Init();
  MX_USART2_UART_Init();
  MX_USART3_UART_Init();
  MX_TIM5_Init();
  /* USER CODE BEGIN 2 */
  // HAL_TIM_PWM_Start(&htim2,TIM_CHANNEL_1);

  //PWM -- CNT = CRR中断
  HAL_TIM_Base_Start_IT(&htim8);

  //PWM输入捕获
  HAL_TIM_IC_Start_IT(&htim3,TIM_CHANNEL_1);
  HAL_TIM_IC_Start_IT(&htim3,TIM_CHANNEL_2);
	HAL_TIM_IC_Start_IT(&htim4,TIM_CHANNEL_1);
  HAL_TIM_IC_Start_IT(&htim4,TIM_CHANNEL_2);

  visual_communicate_init();
  abs_encoder_init();
  
    
  /* USER CODE END 2 */

  /* Infinite loop */
  /* USER CODE BEGIN WHILE */
  while (1)
  {
    /* USER CODE END WHILE */

    /* USER CODE BEGIN 3 */
    //遥控器状态获取 --- 7 10 14
    if(T3_end_flag)
    {
      printf("\r\n定时器3捕获--频率=%.2fHZ,占空比=%.2f%%\r\n",T3_frequency,T3_duty_cycle); // 7 10 14
      if(T3_duty_cycle > 13.5 && T3_duty_cycle < 15 || T3_duty_cycle > 6.5 && T3_duty_cycle < 8)
      {
        object_pos_ctr_flag = 1;
      }
      T3_end_flag = 0;
    }
    if(T4_end_flag)
    {
      printf("\r\n定时器4捕获--频率=%.2fHZ,占空比=%.2f%%\r\n",T4_frequency,T4_duty_cycle); // 7 10 14
      if(T4_duty_cycle > 13.5 && T4_duty_cycle < 15 || T4_duty_cycle > 6.5 && T4_duty_cycle < 8)
      {
        object_pos_ctr_flag = 2;
      }
      T4_end_flag = 0;
    }

    //高度电机加减速控制
    if(object_pos_ctr_flag == 1)
    {
      zero_find_left_flag = zero_find_right_flag = 0;
      if(T3_duty_cycle > 13.5 && T3_duty_cycle < 15 && !high_angctr_state)
      {
        step1_create_t_ctrl_param(SPR*g_step_angle, g_step_accel, g_step_decel, g_set_speed);
        step2_create_t_ctrl_param(SPR*g_step_angle, g_step_accel, g_step_decel, g_set_speed);
        high_angctr_state = 1;
      }
      else if(T3_duty_cycle > 6.5 && T3_duty_cycle < 8 && !high_angctr_state)
      {
        step1_create_t_ctrl_param(-SPR*g_step_angle, g_step_accel, g_step_decel, g_set_speed);
        step2_create_t_ctrl_param(-SPR*g_step_angle, g_step_accel, g_step_decel, g_set_speed);
        high_angctr_state = 1;
      }
      else if(T3_duty_cycle > 9 && T3_duty_cycle < 11.5)
      {
        g_srd1.run_state = STOP;  
        g_srd2.run_state = STOP;
        high_angctr_state = 0;
      }
    }

    // 自动化实现 1s为周期接收视觉发来的数据 ctr_value_flo 单位为厘米
    // 由脉冲数算出累计走过的角度 可能存在丢步的情况
    // 0.1cm以上方动  视觉最大5cm 对应0.032 * 5圈 --0.16圈 --> 0.16 * 1600 = 256脉冲

    //需统计整个高度下的最大脉冲数 作为软限位

    else if(object_pos_ctr_flag == 2)
    {
      //设置向上 为开启自动模式 接收变化量为ctr_value_flo厘米 
      //需保证在下一次视觉变化量发来之前 采茶机在本次变化量控制过程已经完成 即采茶机高度已经响应完成
      if(T4_duty_cycle > 6.5 && T4_duty_cycle < 8 && Ctr_high_M_flag )
      {
          //实际角度：g_step_position1 * 0.225
          targer_circle = (ctr_value_flo * 1000 * 32 / 1000000);
          
          // targer_circle = (fake_value_flo * 1000 * 32 / 1000000);
          input_pulse = (int16_t)(SPR*targer_circle);
          if(abs(input_pulse) >= 1)
          {
            step1_create_t_ctrl_param(input_pulse, g_step_accel * 2, g_step_decel * 2, g_set_speed);
            step2_create_t_ctrl_param(input_pulse, g_step_accel * 2, g_step_decel * 2, g_set_speed);
          }

          Ctr_high_M_flag = 0;
      }

      //向下 待定
      else if(T4_duty_cycle > 13.5 && T4_duty_cycle < 15 && Ctr_high_M_flag ) 
      { 
        
      }

      //归中
      else if(T4_duty_cycle > 9 && T4_duty_cycle < 11.5)
      {
        g_srd1.run_state = STOP;  
        g_srd2.run_state = STOP;
        // visual_angctr_state = 0;
        // g_step_position1 = 0;
        // g_step_position2 = 0; //不能清0 作为无编码器方案 要统计实际脉冲 以防超出硬件高度
      }

      // //软限位 要求超过就停止（无编码器版本） 要求茶机从最低点开始运行
      // if(g_step_position1 > circle_max_pulse)  g_srd1.run_state = STOP;  
      // if(g_step_position2 > circle_max_pulse)  g_srd2.run_state = STOP; 

      //软限位 要求超过就停止（编码器版本）
      if(real_pos_L > abs_encoder_tea_H_max || real_pos_L < abs_encoder_tea_H_min)  g_srd1.run_state = STOP;  
      if(real_pos_R > abs_encoder_tea_H_max || real_pos_R < abs_encoder_tea_H_min)  g_srd2.run_state = STOP; 

      // if(abs(g_step_position1 - (SPR*(float)(100*x/y))) < 5 && abs(g_step_position2 - (SPR*(float)(100*x/y))) < 5) //实际角度达到目标角度
      // {
      //   // visual_angctr_state = 0;

      // }

    }
    
    /***********************************************************************/
    /***********************************************************************/
    /*****************************按钮控制弧度电机***************************/
    /***********************************************************************/
    /***********************************************************************/
    if(HAL_GPIO_ReadPin(RADIAN_M1_2_GPIO_Port, RADIAN_M1_2_Pin) != SET) //按钮状态获取
    {
      step3_create_t_ctrl_param(SPR*(g_step_angle + radian_step_add), g_step_accel + radian_accel_add, g_step_decel + radian_decel_add, g_set_speed + radian_speed_add);
      step4_create_t_ctrl_param(SPR*(g_step_angle + radian_step_add), g_step_accel + radian_accel_add, g_step_decel + radian_decel_add, g_set_speed + radian_speed_add);
    }
    else if(HAL_GPIO_ReadPin(RADIAN_M1_2_R_GPIO_Port, RADIAN_M1_2_R_Pin) != SET)
    {
      step3_create_t_ctrl_param(-SPR*(g_step_angle + radian_step_add), g_step_accel + radian_accel_add, g_step_decel + radian_decel_add, g_set_speed + radian_speed_add);
      step4_create_t_ctrl_param(-SPR*(g_step_angle + radian_step_add), g_step_accel + radian_accel_add, g_step_decel + radian_decel_add, g_set_speed + radian_speed_add);
    }
    else 
    {
      g_srd3.run_state = STOP;
      g_srd4.run_state = STOP;
    }
    
  }

  /* USER CODE END 3 */
}

/**
  * @brief System Clock Configuration
  * @retval None
  */
void SystemClock_Config(void)
{
  RCC_OscInitTypeDef RCC_OscInitStruct = {0};
  RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};

  /** Configure the main internal regulator output voltage
  */
  __HAL_RCC_PWR_CLK_ENABLE();
  __HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE1);

  /** Initializes the RCC Oscillators according to the specified parameters
  * in the RCC_OscInitTypeDef structure.
  */
  RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSE;
  RCC_OscInitStruct.HSEState = RCC_HSE_ON;
  RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
  RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE;
  RCC_OscInitStruct.PLL.PLLM = 4;
  RCC_OscInitStruct.PLL.PLLN = 168;
  RCC_OscInitStruct.PLL.PLLP = RCC_PLLP_DIV2;
  RCC_OscInitStruct.PLL.PLLQ = 4;
  if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)
  {
    Error_Handler();
  }

  /** Initializes the CPU, AHB and APB buses clocks
  */
  RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK
                              |RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2;
  RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
  RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
  RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV4;
  RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV2;

  if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_5) != HAL_OK)
  {
    Error_Handler();
  }
}

/* USER CODE BEGIN 4 */
void HAL_TIM_IC_CaptureCallback(TIM_HandleTypeDef *htim)
{
	  /*PWM 信号的第1个上升沿时，定时器产生中断，计数器，CCR寄存器被复位
    当下降沿到来时，IC2 会捕获，对应的是脉冲宽度测量，但不会产生中断。当
	  二个上升沿时，IC1会捕获，对应的是周期宽度测量*/
    if(htim == &htim3)
    {
      T3_CCR1=HAL_TIM_ReadCapturedValue(&htim3, TIM_CHANNEL_1);
      if(T3_CCR1!=0)
      {
        T3_CCR2=HAL_TIM_ReadCapturedValue(&htim3, TIM_CHANNEL_2);
        T3_frequency=(float)cnt_clk/(T3_CCR1+1);
        T3_duty_cycle=(float)(T3_CCR2+1)*100/(T3_CCR1+1);	
        T3_end_flag=1;
      }
      else
      {
        T3_frequency=0;
        T3_duty_cycle=0;
      }
    }

    else if(htim == &htim4)
    {
      T4_CCR1=HAL_TIM_ReadCapturedValue(&htim4, TIM_CHANNEL_1);
      if(T4_CCR1!=0)
      {
        T4_CCR2=HAL_TIM_ReadCapturedValue(&htim4, TIM_CHANNEL_2);
        T4_frequency=(float)cnt_clk/(T4_CCR1+1);
        T4_duty_cycle=(float)(T4_CCR2+1)*100/(T4_CCR1+1);	
        T4_end_flag=1;
      }
      else
      {
        T4_frequency=0;
        T4_duty_cycle=0;
      }
    }
}

void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
{
  static uint16_t count = 0;
  static uint8_t k = 0;
  if(htim == &htim5)  //5ms
  {
    count ++;
    if(count % 10 == 0)
    {
        read_encoder_pos();
    }
  
    if(count % 100 == 0)
    {
        Tovisual_dataPack();
    }

    if(count >= 600) //3s变化一次
    {
        fake_value_flo = fake_value_flo_arr[k];
        count = 0;
        Ctr_high_M_flag = 1;
        k ++;
        if(k >= 6) k = 0;
    }
        
  }
}


void HAL_UART_TxCpltCallback(UART_HandleTypeDef *huart)
{
    if(__HAL_UART_GET_FLAG(&huart2, UART_FLAG_TC))
    {
        __HAL_UART_CLEAR_FLAG(&huart2, UART_FLAG_TC);
    }

    if(__HAL_UART_GET_FLAG(&huart3, UART_FLAG_TC))
    {
        __HAL_UART_CLEAR_FLAG(&huart3, UART_FLAG_TC);
    }
}

/* USER CODE END 4 */

/**
  * @brief  This function is executed in case of error occurrence.
  * @retval None
  */
void Error_Handler(void)
{
  /* USER CODE BEGIN Error_Handler_Debug */
  /* User can add his own implementation to report the HAL error return state */
  __disable_irq();
  while (1)
  {
  }
  /* USER CODE END Error_Handler_Debug */
}

#ifdef  USE_FULL_ASSERT
/**
  * @brief  Reports the name of the source file and the source line number
  *         where the assert_param error has occurred.
  * @param  file: pointer to the source file name
  * @param  line: assert_param error line source number
  * @retval None
  */
void assert_failed(uint8_t *file, uint32_t line)
{
  /* USER CODE BEGIN 6 */
  /* User can add his own implementation to report the file name and line number,
     ex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */
  /* USER CODE END 6 */
}
#endif /* USE_FULL_ASSERT */
